% -------------------------------------------------------------------------
% state dynamics
%
% dx = fy_(t , x , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% t             [1x1 double]           time                [ TU ]        
% x             [14x1 double]          state variables     [  -  ]
% auxdata       [structure]            parameters          [  -  ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% dx            [14x1 double]          vector field         [  -  ]
%
% External functions called:
% -------------------------------------------------------------------------
% None
%
% Copyright(C) 2015/06/30 by Chen Zhang, 
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------

function dx = fy(t , x , auxdata)
mu = auxdata.mu;
Tmax = auxdata.Tmax; 
c = auxdata.c;

% ------------------------------------------------------------------------
r3 = ((mu + x(1) - 1)^2 + x(2)^2 + x(3)^2)^(3/2);
d3 = ((mu + x(1))^2 + x(2)^2 + x(3)^2)^(3/2);
r5 = r3^(5/3);
d5 = d3^(5/3);

pv_two = (x(11)^2 + x(12)^2 + x(13)^2);
pv_mag = pv_two^(1/2);

% calculate switching function 
Sk = -pv_mag * auxdata.c / x(7) - x(14); 
if Sk < 0; % turn engine 'On'
    u = 1;
else % turn engine 'Off'
    u = 0;
end

% ------------------------------------------------------------------------
% combined dynamic equation
Z5 = (mu + x(1) - 1);
Z6 = (mu + x(1))*(mu - 1);
Z7 = (2*mu + 2*x(1));
Z8 = (2*mu + 2*x(1) - 2);

dx = [x(4);
    x(5);
    x(6);
    x(1) + 2*x(5) - (mu*Z5)/r3 + (Z6)/d3 - (Tmax*u*x(11))/(x(7)*pv_mag);
    x(2) - 2*x(4) - (mu*x(2))/r3 + (x(2)*(mu - 1))/d3 - (Tmax*u*x(12))/(x(7)*pv_mag);
    (x(3)*(mu - 1))/d3 - (mu*x(3))/r3 - (Tmax*u*x(13))/(x(7)*pv_mag);
    -(Tmax*u)/c
    - x(11)*((mu - 1)/d3 - mu/r3 + (3*mu*Z8*Z5)/(2*r5) - (3*Z7*Z6)/(2*d5) + 1) - x(12)*((3*mu*x(2)*Z8)/(2*r5) - (3*x(2)*Z7*(mu - 1))/(2*d5)) - x(13)*((3*mu*x(3)*Z8)/(2*r5) - (3*x(3)*Z7*(mu - 1))/(2*d5));
    x(13)*((3*x(2)*x(3)*(mu - 1))/d5 - (3*mu*x(2)*x(3))/r5) - x(12)*((mu - 1)/d3 - mu/r3 - (3*x(2)^2*(mu - 1))/d5 + (3*mu*x(2)^2)/r5 + 1) - x(11)*((3*mu*x(2)*Z5)/r5 - (3*x(2)*Z6)/d5);
    x(12)*((3*x(2)*x(3)*(mu - 1))/d5 - (3*mu*x(2)*x(3))/r5) - x(13)*((mu - 1)/d3 - mu/r3 - (3*x(3)^2*(mu - 1))/d5 + (3*mu*x(3)^2)/r5) - x(11)*((3*mu*x(3)*Z5)/r5 - (3*x(3)*Z6)/d5);
    2*x(12) - x(8);
    - 2*x(11) - x(9);
    -x(10);
    -(Tmax*u*pv_mag)/x(7)^2];

end
%--------------------------------------------------------------------------
